#include <iostream>
#include <vector>
#include "Sphere.hpp"  // Ensure this includes the Sphere_Curve_Fitting function

int main() {
    // Define a set of points on the unit sphere
    // These points are for testing purposes and should lie on a unit sphere
    std::vector<double> x = {0.5, 0.7, 0.3, 0.5, -0.6};  // x coordinates
    std::vector<double> y = {0.6, 0.1, -0.6, 0.7, 0.1}; // y coordinates
    std::vector<double> z(x.size(), 0.0); // Initialize z values

    // Calculate the z values assuming these points lie on the unit sphere
    for (size_t i = 0; i < x.size(); ++i) {
        z[i] = std::sqrt(1 - x[i]*x[i] - y[i]*y[i]);  // z = sqrt(1 - x^2 - y^2) for unit sphere
    }

    // Output original points
    std::cout << "Original Points (x, y, z):\n";
    for (size_t i = 0; i < x.size(); ++i) {
        std::cout << "(" << x[i] << ", " << y[i] << ", " << z[i] << ")\n";
    }

    try {
        // Perform sphere curve fitting
        auto result = Sphere_Curve_Fitting(x, y, z);

        // Extract the resulting x, y, z sphere points from the result
        const std::vector<double>& x_sphere_spline = result[0];
        const std::vector<double>& y_sphere_spline = result[1];
        const std::vector<double>& z_sphere_spline = result[2];

        // Output the resulting fitted sphere spline points
        std::cout << "\nFitted Sphere Spline Points:\n";
        for (size_t i = 0; i < x_sphere_spline.size(); ++i) {
            std::cout << "(" << x_sphere_spline[i] << ", "
                      << y_sphere_spline[i] << ", "
                      << z_sphere_spline[i] << ")\n";
        }

    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << "\n";
    }

    return 0;
}
